//find_heel_strike.hpp
#include "behaviortree_cpp/action_node.h"
#include "utils/load_data.hpp"

// ANSI 颜色代码
#define COLOR_RED     "\033[31m"
#define COLOR_GREEN   "\033[32m"
#define COLOR_YELLOW  "\033[33m"
#define COLOR_BLUE    "\033[34m"
#define COLOR_MAGENTA "\033[35m"
#define COLOR_CYAN    "\033[36m"
#define COLOR_RESET   "\033[0m"    


class FindHeelStrike : public BT::StatefulActionNode
{
public:
  explicit FindHeelStrike(const std::string &name,  const BT::NodeConfig& config) : BT::StatefulActionNode(name, config){cnt = 0;};
        BT::NodeStatus onStart() override;
        BT::NodeStatus onRunning() override;
        void onHalted() override{
            //处理中断
        }
        // 添加静态端口声明方法
        static BT::PortsList providedPorts() {
            return BT::PortsList({
                BT::InputPort<sensorData>("sensor_input", "Description of input"),
                BT::InputPort<uint64_t>("T_fid", "Tof distance from positive to negative"),
                BT::BidirectionalPort<std::shared_ptr<std::vector<uint64_t>> >("mean_T_hs_potential_region","mean_T_hs_potential_region"),
                BT::InputPort<uint64_t>("T_to", "Time point of last toe off"),
                BT::BidirectionalPort<std::shared_ptr<std::vector<uint64_t>> >("mean_swing_duration","Time of mean_swing_duration"),
                BT::OutputPort<uint64_t>("T_hs", "Time point of heel strike")
            });
        }
        //简单 vector 版本实现，增量式求均值
        void new_value_update_mean(uint64_t new_value, std::vector<uint64_t>& mean_history, size_t window_size = 5000);
private:
        int cnt;
        uint64_t current_millis_=0;
        uint64_t T_fid_=0.0;
        uint64_t last_T_hs_potential_region = 0.0;
        uint64_t T_hs_potential_region_= 0.0;
        std::vector<uint64_t> mean_T_hs_potential_region_;
        std::shared_ptr<std::vector<uint64_t>>  mean_T_hs_potential_region_ptr = nullptr;
        uint64_t swing_duration_=0.0;
        std::vector<uint64_t> mean_swing_duration_;
        std::shared_ptr<std::vector<uint64_t>>  mean_swing_duration_ptr = nullptr;
        uint64_t tof_distance_=0.0;
        sensorData tmp_sensor_data_filtered_;
        double F_gy_ = 0.0, F_gy_last_=0.0;
        uint64_t T_hs_ = 0, T_to_ = 0, T_hs_realtime_ = 0;
        std::vector<double> buffer_gyr_;
        std::vector<uint64_t> buffer_time_label_;
        double R_gy = 0.0;
        size_t R_gy_index_=0;
        void print_all_outports(bool print_switch);
        void print_all_inports(bool print_switch);
        void print_vector(const std::vector<uint64_t>& vec, const std::string& name);
        void print_var(const uint64_t& var, const std::string& name);
}; // 确保这里有分号

